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Abstract 

We propose in this paper a constructive procedure that transforms locally, 
even at singular configurations, the kinematics of a car towing trailers into 

a ■ 

Kumpera-Ruiz normal form. This construction converts the nonholonomic 
motion planning problem into an algebraic problem (the resolution of a sys- 
tem of polynomial equations), which we illustrate by steering the two-trailer 
system in a neighborhood of singular configurations. We show also that the 
n-trailer system is a universal local model for all Goursat structures and that 
all Goursat structures are locally nilpotentizable. 

Keywords: Nonholonomic control systems, feedback equivalence, Kumpera- 
Ruiz normal forms, Goursat normal form, car with trailers, nilpotent Lie 
algebras, nonholonomic motion planning. 
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Introduction 



The nonholonomic motion planning problem for a car towing trailers has been one 
of the most widely studied problems in nonlinear control theory during the last ten 
years. The controllability of this system was proved by |LaumonH| ( |1991| ). Since 
then many important results have been obtained, including solutions of the motion 
planning and stabilization problems — see the papers ( [Fliess et al.| , |1995| ; [Rouchori 
et al.| , |1993| ; |Jiang and INijmefjer] , |1999| ; |Samson| , |1995| ; |S0rdalen| [I993| ; [l'eel et aT 



1 9 9 5| ; Tilbury et al.[ |1995| ), the book ( [Laumono 1 ] , |1997| ), and references given there 



with the main emphasis put on regular configurations. For singular configurations 
much less results are available (see, however, |Uheaito and Mormul , 1999 ; J can . 
1996| ; Pasillas-Lepine and Responded , |1998| ; [Vendittelli, Laumond and Oriolo| , |1998 



although it is clear that for some steering situations, like turning and going back in 
a narrow street, it is necessary to cross the singular locus. The aim of this paper 
is to show that theoretical results of ( |Cheaito and Mormul| , |1999| ; |Kumpera and 
Ruiz; , |1982| ; Pasillas-Lepine and Responded , |1999b| ) can be applied to this problem. 



We illustrate the theory with a concrete example: the two-trailer system. 

The paper is organized as follows. In Section p] we introduce Goursat struc- 
tures and define Kumpera-Ruiz normal forms for Goursat structures. In Section Q 
we show that any Kumpera-Ruiz normal form is defined by a pair of vector fields 
that generate a nilpotent Lie algebra thus proving that any Goursat structure is 
locally feedback nilpotentizable. In Section ^| we recall the nonholonomic model of 
a car towing trailers. Then we show how to transform the rz-trailer system into a 
Kumpera-Ruiz normal form and we prove a surprising result: any Goursat struc- 
ture is locally equivalent to the ^-trailer system around a well chosen point of its 
configuration space. An alternative proof of this fact has been proposed by [Mont- 
gomery and Zhitomirski^ ( |1999| ). In Section [|, we briefly discuss how transforming 



the n-trailer system to a Kumpera-Ruiz normal form simplifies the motion planning 
problem by reducing it to an algebraic problem of solving a system of polynomial 
equations. We illustrate this approach in Section |5] by applying it to the motion 
planning problem through singular configurations for the two-trailer system. Pre- 
liminary results have been published in ( |Pasillas-Lepine and Responded , |1999a| ). 
Acknowledgments: The authors would like to thank Henk Nijmeijer for interest- 
ing comments on the paper. 
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1 Goursat Structures and Kumpera-Ruiz Normal 
Forms 



A rank-A; distribution V on a smooth manifold M is a map that assigns smoothly 
to each point pinMa linear subspace T>(p) C T P M of dimension k. Such a field of 
fc-planes is locally spanned by k pointwise linearly independent smooth vector fields 
fx,... , f k , which will be denoted by V = (f u ... , f k ). 

Two distributions T> and T> defined on two manifolds M and M, respectively, 
are equivalent if there exists a smooth diffeomorphism (p between M and M such 
that {ip*T>)(p) = T>(p), for each point p in M. Clearly, two distributions T> and D 
are locally equivalent if and only if the corresponding control systems 



x 



X 



i=i 



i=i 



are locally equivalent via a state static feedback. 

The derived flag of a distribution T> is the sequence of modules T>^ C T>^ C • ■ ■ 
defined inductively by = V and = V® + [V^,V% for % > 0. 

Definition 1.1 A Goursat structure on a manifold M of dimension n > 3 is a 
rank-two distribution D such that, for < i < n — 2, the elements of its derived 
flag satisfy dimPW(p) =2 + 2, for any point p in M. 

Any Goursat structure on a manifold M of dimension n is equivalent, in a small 
enough neighborhood of any point of an open and dense subset of M, to the distri- 
bution spanned by 



a 



+ ■ 



+ x „_9_ + JL 



n dXn-1 ' ' 8X2 

which is called Goursat normal form or chained form. We refer the reader to ( Bryant 
let all , |1991| ; |Kumpera and Ruizj , |1982| ; |Martin and Rouchonl , |1994| ; |Murrayj |1994j) 
for additional information about this normal form. If at a given point a Goursat 
structure can be converted into Goursat normal form then this point is called regular; 
otherwise, it is called singular. An elegant characterization of regular points, based 
on the concept of growth vector, can be found in the work of |Murray| ( |1994| ) . 

For Goursat structures, the first who observed the existence of singular points 
were Giaro, Kumpera and Ruiz ~J l 978 J~ This initial observation has led Kumpera 



and Ruiz ( p. 982 ) to write their innovative article, where they introduced the normal 



forms that we consider in this section. 

We start with the Pfaff-Darboux and Engel normal forms, given respectively 
on R 3 and R 4 by the pairs of vector fields k 3 = (/cf , k 3 ,) and k 4 = (k 4 , nf), defined 
respectively by 

a 



and 



r 4 _ a 

1 9x4 



Loosely speaking, we can write 

,a = a 

i^*4 — , . . 1 i^-3 

In order to make this precise we will adopt the following natural convention. Con- 
sider a vector field 

i=l 

on R™" 1 equipped with coordinates (xi,... ,x n _i). We can lift f n ~ l to a vector 
field, denoted also by / n_1 , on R n equipped with coordinates (xi, . . . ,x n _i,x n ) by 
taking 



n-l 

r-i = j:fr\x 1 ,...,x n _ 1 )£- i +o.£- n . 

i=l 



That is, we lift f n by translating it along the x n -direction. 

Notation 1.2 From now on, in any expression of the form k% = q;(x)k" _1 + 
/?(x)k2 _1 , the vector fields k™ -1 and k^ -1 should be considered as the above de- 
fined lifts of k™ -1 and k^ -1 , respectively. 

Let ) denote a pair of vector fields on R n 1 . A regular prolon- 

gation, with parameter c n , of /t n_1 , denoted by K n = ^^(/t™" 1 ), is a pair of vector 
fields K n = (k™, Kg) defined on R n by 

^2 = v^n CnJ^l 4"" K 2 ' 
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where c n belongs to R. The singular prolongation of K n 1 , denoted by /« r ' 
is the pair of vector fields K n = Kg) defined on by 



9 

n-1 



(2) 



Definition 1.3 For n > 3, a pair of vector fields k" on K n is called a Kumpera-Ruiz 
normal form if it is given by K n = o n 
equals either S or R Ci , for some real constants q. 



_ 3 o- ■ ■ocr 1 (fi; 3 ), where each <7j, for 1 < i < n — 3, 



Note that our definition differs slightly from that of |Cheaito et al.| (|1998| ). Firstly, 
we do not ask the coordinates to satisfy x(p) = 0, where p is the point around which 
we work. Secondly, we consider the models S(k 3 ) and R c (k 3 ), which are equivalent to 
Ro(k 3 ), as being Kumpera-Ruiz normal forms. The following result of Kumpera and 
Ruiz| ( |1982|) (see also |Cheaito and Mormul| , |1999| ; |Cheaito et al.| , |1998| ; [Montgomery 
|and Zhitomirskii| , |1999| ; |Pasillas-Lepine and Respondek| , |1999b| ) shows clearly the 
interest of their normal forms. 

Theorem 1.4 (Kumpera-Ruiz) Any Goursat structure is locally equivalent to a 
distribution spanned by a Kumpera-Ruiz normal form. 

This result implies that locally, even at singular points, Goursat structures do not 
have functional invariants. This make them precious but rare and distinguish them 
from generic rank-two distributions on n-manifolds, which do have local functional 
invariants when n > 5. Moreover, as we will see in the next Section, this result 
implies that any Goursat structure is locally feedback nilpotentizable. 



2 Nilpotentization 



Let us recall the following standard concepts (see e.g. Fulton and Harris , 1991 ) 



Let g be a Lie algebra. A Lie subalgebra of g is a linear subspace f) C g such that 
[f), f)] C f}. An ideal of g is a Lie subalgebra i such that [i,g] C i. The lower central 
series of a Lie algebra g is the sequence of ideals Dq(q) D Di(g) D ■ • ■ defined by 

D (fl) = and Xt fc (fl) = [0,©fc_i(fl)], for k > 1. 
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In other words, for a fixed k > the ideal Dk(s) 1S the subspace generated by 
the elements of g that can be expressed as a left-iterated Lie bracket of the form 
[eo, [ei, . . . [ei-i, e{\ . . . ]], where I > k and eo, . . . , e-i are elements of g. Observe that 
unlike the Lie and derived flags, which increase, the lower central series decreases. 

A Lie algebra g is nilpotent if there exists some integer r such that 2? r (fl) — 0; 
the smallest such integer is called the nilindex of g. It is clear that a Lie algebra g 
is nilpotent if and only if there exists some integer r such that, for every sequence 
e , . . . , e r of elements of g, we have 

[e , [ei, . . . [e r _i,e r ] ...]]= 0. 

Denote ad eo (ei) = [eo, e\], for all eo and e± in g . For each element eo in g, the map 
ad eo is linear. The following result is standard (see e.g. Fulton and Harris] , |1991| ). 



Lemma 2.1 (Engel) A finite dimensional Lie algebra g is nilpotent if and only if 
ad eo is nilpotent for each element eo in g. 

For any subspace V of a given Lie algebra g, not necessarily of finite dimension, 
define the sequence of subspaces C C • • ■ by 

V {0) = V and V {k+l) = V ik) + [V {k \ V {k) }. 

The Lie algebra generated by a subspace V C g is the infinite sum 

C(V) = v {0) + v m + ■■■ + v ik) + ■■■ , 

which is clearly a Lie subalgebra of g. 

A distribution is said to be nilpotentizable if we can chose a family of vector fields 
that span the distribution and generate a nilpotent Lie algebra of finite dimension. 
The class of nilpotentizable distributions is particularly important in control theory, 
because for them, for instance, a general motion planning algorithm exists (see e.g. 
Hermes| , |1986| ; [Lafierriere and Sussmann] , |1992| ; [Murray| |1994| ). The following result 



states that any nonholonomic control system with two controls that generates a 
Goursat structure is locally feedback equivalent to a system whose Lie algebra is 
finite dimensional and nilpotent. 

Theorem 2.2 Goursat structures are locally nilpotentizable. In fact, any Kumpera- 
Ruiz normal form generates a nilpotent Lie algebra that has finite dimension. 
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Proof: By Theorem |1.4| , any Goursat structure on a manifold of dimension n is 
locally equivalent to a Kumpera-Ruiz normal form k?;) centered at zero. It thus 
suffices to prove that ( ) generates a nilpotent Lie algebra. 

Let (n™, k 2 ) be any Kumpera-Ruiz normal form on M n . Denote by f) n the Lie 
algebra generated by and k 2 - The main argument of the proof is to show that f) n 
is a Lie subalgebra of a nilpotent Lie algebra g n of finite dimension. Thus \) n is 
nilpotent, since any subalgebra of a nilpotent Lie algebra is itself nilpotent. 

Let us proceed by induction on n. For n = 3 it is clear that f)3 is nilpotent, 
since (33 is the three-dimensional Heisenberg algebra. Assume that f) ra __i is nilpotent 
and of finite dimension. We will prove that this assumption implies that f) n is 
nilpotent and of finite dimension. 

Put V ). From the definition of V^ -* and and the definition 

of Kumpera-Ruiz normal forms, we have 

V (0) = span R (/t™, k%) and C span R (K™, k%, f)„_i). 

Define the sequence E^\ for i > 0, of subspaces of the Lie algebra of all polynomial 
vector fields on W 1 , by taking = span R (K™ , k^,) and 

E (i) = E d-i) + p^lx^Q^fi^), for i > 1, (3) 

where Pfc[a; n ] denotes the vector space of polynomials with real coefficients of the 
variable x n that have degree d < k, and P 2 i-i [a; n ]Dj_i(f) n _i) denotes the subspace 
obtained by taking linear combinations, with coefficients in P 2 i-i[x n ], of vectors 
fields in 2)i_i(fj n _i). 

Denote by r the nilindex of t) n -i an d put g n = E^ r >. The rest of the proof is 
a direct consequence of the following Lemma, which shows that f) n , the Lie alge- 
bra generated by V^°\ is contained in the nilpotent Lie algebra g n , that has finite 
dimension □ 

Lemma 2.3 We have the following properties ofV^ and E^ : 

(i) + [E®,E®] C 

(ii) V® C 

(iii) E^ = 

(iv) E^ r > is a Lie algebra of finite dimension; 

(v) E^ is nilpotent. 
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Proof: Let us denote Wq = span M (K™, and 

Wl 1 = P a <-x[x n ]2)i-l(&n-l), for Z > 1. 

First Item. This Item can be proved by induction on i. We clearly have + 
[£ (0) ,£ (0) ] C Assume that Item (i) is true up to i. We are going to prove that 
then it is also true for i + 1. Indeed, we have 

E® = W£ + W? + ■ • ■ + W t n . 

Since, by the induction assumption, E^' + [E^ 1 ', E^'] C E^ l+1 \ what remains to prove 
is that [Wj 1 , W-+i] C E( l+2 \ for < j < i + 1. But this last relation results directly 
from the definitions of lDj+i(l) n _i) and P 2 i + 1 [ a; «]' and from standard properties of 
the Lie bracket. 

Second Item. This Item is a direct consequence of Item (i) and the relation 
y(o) = #(o). 

T/wni Item. Since the nilindex of f) n _i is r, the sequence (|]) stabilizes at 
EU = E<r-V + P 2 r-i[x n ]® r - 1 (t )n - 1 ). 
Indeed, we have E^ r+1 ^ = + P 2 r[x n \8 r (f) n _i) and thus £( r+1 ) = SW, since 

S)r(^-l) = 0. 

Fourth Item. Items (i) and (iii) imply + [E^ r \ E^} C £ (r) , that is £ (r) 
is closed under Lie brackets. But since, by construction, to the finite dimensional 
vector space we add (at each step) a finite dimensional vector space (recall that 
f) n _i has finite dimension), the dimension of E^ is finite. 

Fift Item. It is a direct consequence of Engel's Lemma that E^ r > is nilpotent, 
because ad eo is nilpotent for any eo in E^ r \ Indeed, observe that for any Wi G WJ 1 
and Wj G Wj 1 , such that i < j, we have [wi,Wj] G W™ +1 . 

□ 



3 The N- Trailer System 



The kinematical model for a unicycle-like mobile robot towing n trailers such that 
the tow hook of each trailer is located at the center of its unique axle is usually 
called, in control theory, the n-trailer system — see ( Fliess et al. , 1995 ), flJcan . 
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BMP , ( [Laumondl , [TWTD , (|Laumond| , [TW7|) , ( |Rouchon et aT| , POT|) , (|Samsorj ^9"93|) , 
( |S0rdalen| , |1993| ), ( |Teel et aT| , |1995| ), ( [Tilbury et al.| , |1995|) , and references therein. 
For simplicity, we will assume that the distances between any two consecutive trailers 
are equal. 

We give here an inductive definition of the n-trailer. This definition already ap- 
pears in flJean| , |1996|) and reminds the one given in the previous section for Kumpera- 



Ruiz normal forms. To start with, consider the pair of vector fields (t^t^) on 
M 2 x S 1 , equipped with coordinates (^1,^2,^0)1 that describe the kinematics of the 
unicycle-like mobile robot towing no trailers: 



r- 







d 



sin 



90)4 + 008(^)4. 



The n-trailer system is defined by applying successively a sequence of prolongations 
to this mobile robot. In order to do this, suppose that a pair of vector fields r n ~ 1 = 
(r™ _1 , T2~ l ) on IR 2 x (S 1 ) 11 associated to the mobile robot towing n — 1 trailers has 



been defined. The pair of vector fields r n 
n-trailer system is given by 



on 



x (S 



l\n+l 



defining the 



TV 



T" 



sin 



On-lK' 1 + C0S( 



0,, 



where the coordinates £1 and £2 represent the position of the last trailer, while the 
coordinates Oo, . . . ,9 n represent, starting from the last trailer, the angles between 
each trailer's axle and the £i-axis. Observe that this definition should be understood 
in the sense of Notation |1.2| . Mechanically, to prolongate the n-trailer means to add 
one more trailer to the system. It is easy to check that the n-trailer system is a 
Goursat structure. 

A direct consequence of Kumpera-Ruiz's theorem is that, in a small enough 
neighborhood of any point of its configuration space, in particular at any singular 
configuration, the n-trailer can be converted into Kumpera-Ruiz normal form. One 
of the main results of this paper is to describe this conversion explicitly. For regular 
configurations, our result gives the transformations proposed in (|S0rdalen| , |1993| ) and 
([Tilbury et al.| , |1995| ); for singular configurations, a new kind of transformations is 
obtained. 
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The n-trailer system can also be written as the control system 

C = T?(0 «1 + 7?(C) V 2 , (4) 

where ( = (£1, £2, 9o, On)- Recall that a feedback transformation of ((D is a change 
of controls of the form 

Ux = u(() v x + 77(C) v 2 ^ 
u 2 = A(C) vi + //(C) v 2 , 

where the smooth functions u, A, 77, and fi are such that (z//i — A?7)(-) 7^ 0. 
Fix a point p of M 2 x (S' 1 ) n+1 given in C-coordinates by 

c(p) = c p = (e?,a,c-,^)- 

In order to convert, locally at p, the n-trailer into a Kumpera-Ruiz normal form we 
look for a local change of coordinates 

(ari, . . . , x n+3 ) = n (fi, f 2 , 9 ,... , n ) 

and a local triangular feedback transformation of the form u\ = v n (C) v i + ^(C) v 2 
and u 2 = /i n (C) u 2 that bring (f|) into 

X = K™(x) Mi + Kg (x) U 2 , (6) 

where x = (xi, . . . , x n+ z). In other words, we ask the change of coordinates and the 
feedback transformation to satisfy 

(7) 



«2 -^nTf + fe 



2 



or, equivalently, 



^(r 2 n ) = (77 n o^)^ +3 + (MnoV n )^ +3 



(8) 



where ^ n = (0 n ) _1 denotes the inverse of the local diffeomorphism n , both ^ n (C p ) 7^ 
and fi n {C p ) 7^ 0, and the inverse feedback transformation v\ = v n (C) Ui + Vn{C) u 2 
and t> 2 = ju n (C) M2 is obviously given by 

i/ n = — , T] n = , and fi n = — . (9) 
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Observe that we not demand the x-coordinates to be centered at p, and thus the 
point x(p) = (<p n o Q(p) will be, in general, different from zero. 

To start our construction, take x\ = £2 and X2 = £i- If P 7^ ±7r/2 mod 2tt take 
X3 = tan(6 l ), fio = cos(6 l o), uq = sec 2 (6'o), and 770 = 0. If 6$ = ±ir/2 mod 2n take 
x 3 = cot(^o), A^o — s i n (^o); u q — ~ c sc 2 (^o)> an d Vo = 0- Denote = sin(6 , i — 
and q = cos(6'j — for < i < n. Moreover, denote by Lja the Lie derivative 

of a function a along a vector field /. 

Now, consider the sequence of smooth functions defined locally, for 1 < i < n, 
by either 

SiVi-i + CiTli-i 
x i+3 — 

Cifk-i 

fJ'i Cj/^i— 1 

v i = L r i Xj + 3 
Vi = Lr* X i+3 

when 9 P — 6 P _ l ^ ±7r/2 mod 2ir (regular case) or by 

x i+3 — : 

Hi = SiVi-i + CiTJi-i 

when #f — 6 l f_ 1 = ±7r/2 mod 27r (singular case). It is easy to prove that, for 
< i < n, the transformations <p l defined by </> 4 (£i, £2, Qq, . . . , 6i) = (x\, . . . , Xi + %) 
are smooth changes of coordinates around Pi and that, moreover, we have both 
u i(C Pi ) 7^ anc I f*i{( Pi ) 0) where pj denotes the projection of p on M 2 x (S' 1 ) t+1 . 

Theorem 3.1 For n > 0, the diffeomorphism <p n and the feedback transformation 
(vniVn-, A*n) satisfy (H), and t/ius convert the n-trailer system into a Kumpera-Ruiz 
normal form. 

Proof: We will prove that the relation @ holds for n > by induction on the 
number n of trailers. Relation (]8|) is clearly true for n = 0. Assume that it holds 

11 



(10) 



for n — 1 trailers. In this case we have 

flrvr 1 ) = (^-lof- 1 )^ 2 

The inductive definition of the n-trailer gives 

_n _ _d_ 

r 2 " = sin(0 n - On-Jr?- 1 + cos(6 n - ^-iK" 1 - 

Let n = (4 >n ~ 1 , <p n +z) T be a diffeomorphism of IR n+3 such that n_1 depends 
on the first n + 2 coordinates only. Let / be a vector field on M n+3 of the form 
/ = ocf( n ~ l > + fn+3, where a is a smooth function on R n+3 , the vector field /( n_1 ) is 
the lift of a vector field on W a+2 (see Notation |1.2j ), and the only non-zero component 
of / n+ 3 is the last one. Then we have 



flif) = («O^)0r 1 (/ (n - 1) ) + ((L/0n + 3)oV n ) 



d 



dx n+ 3 ' 



Note that the vector field 0™ 1 (f ( - n is lifted along the x n+ 3-coordinate, which is 
defined by <fi n +3- 



In the regular case we take a regular prolongation. In this case, relation (12) 
gives: 

= ((s n z/ n _! + c n ri n _ x ) o V n ) < +2 + ((cnHn-i) o i) n ) K™ +2 + (T] n o ^"K +3 
= ( Vn o ^"K +3 + (c^/in-x o V n ) x [( ^/tT" ° ^) K i +2 + K 2 + 

In the singular case we take the singular prolongation and, by (p~2"D, we obtain: 



n\ n+3 



n+2\ 
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Moreover, in both cases, we have 



It follows that, both in the regular and in the singular case, relation (^) holds for 
n > 0. □ 

Reversing the construction given in the previous proof leads to the following sur- 
prising result flPasillas-Lcpinc and R.cspondek , 19981 ), which states that the n-trailer 



system is a universal local model for all Goursat structures — see also ( Montgomery 
and Zhitomirskii| , |1999| ) and flPasillas-Lepine and Responded , |1999b| ). 



Theorem 3.2 Any Goursat structure on a manifold M of dimension n + 3 is equiv- 
alent, in a small enough neighborhood of any point q in M, to the n-trailer considered 
around a suitably chosen point p of its configuration space R 2 x (5 a ) n+1 . 

Proof: By Theorem [O], our Goursat structure is, in a small enough neighborhood 
of any point q in M, equivalent to a Kumpera-Ruiz normal form K n+3 . Denote by 
V = (yi, ■■■ , Vn+s) the coordinates of K n+3 and put (yf, ... , y q n+z ) = y(q). 

Recall that, by definition, the pair of vector fields K n+3 is given by a sequence of 
prolongations k 1 = o"j_ 3 o- • ■ocr 1 (n 3 ) J where <x,- belongs to {R C] , S}, for 1 < j < i — 3 
and 3 < i < n + 3. We call a coordinate yi such that k 1 = ^(k* -1 ) a singular 
coordinate, and a coordinate yi such that k 1 = -R c (k 1_1 ) a regular coordinate. It 
follows from the proof of Theorem |1.4| - see ( pheaito and Mormul| , |1999| ) and 



(|Pasillas-Lepine and Respondek| |1999b| ) — that for all singular coordinates we have 



y\ = 0; but for regular coordinates, the constants y\ can be arbitrary real numbers. 

To prove the theorem, we will define a point p of M. 2 x (S 11 )™" 4 " 1 whose coordi- 
nates ((p) = (£i, £i, #o> • • ■ ' ®n) satisfy {x o Q{p) = y(q), where x and ( denote 
the coordinates used in the Proof of Theorem |3.1| . First, put the axle of the last 
trailer at (y%,yl) and take 6$ = arctan(yg). Compute x 3 = tan(6*o), /io = cos(6 , ), 
uo = sec 2 (#o) ; an d rjo = 0. Then, take for i = 1 up to n, the following values for the 
angles Q\ mod 2tt. If the coordinate y i+3 is singular then put 6\ = Q\_ x + tt/2 and 
compute the coordinate 2^+3 and the smooth functions /i,, z/j, and r\i using ([TT|) . If 
yi + 3 is regular then put 

C = arctanf ^- l( ^H"^ l(P) VC 1 
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and compute the coordinate x i+3 and the smooth functions 2^, and rji using (|T0D . 
By Theorem |3.1| , the coordinates io( convert the n-trailer into a Kumpera-Ruiz 
normal form. By the above defined construction, this normal form has the same 
singularities as K n+3 and is defined around the same point of M n+3 (if we translate 
the coordinates in order to center them then these Kumpera-Ruiz normal forms 
have the same constants in the regular prolongations). Hence, the diffeomorphism 
o o y gives the claimed equivalence. □ 



4 Motion Planning 



As we have proved in Section |2|, the Lie algebras generated by Kumpera-Ruiz nor- 
mal forms are nilpotent. This property is fundamental (see e.g. |Lafferriere and 
[Sussmanh] , |1992| ; |Lfu| , |1997[ ) because it allows to solve the nonholonomic motion 
planning problem in the case of Goursat structures. But Kumpera-Ruiz normal 
forms have also an other interesting property: they are "triangular" — in the sense 
of Murray and Sastry (1993), see also ( [Marigo , |1999j ). Indeed, it follows directly 



from their construction that they give a control system x = Ki(x) u\ + nV, (x) U2 that 



can be written — see flCheaito and Mormul| , |1999|) , QCheaito et al.| , |1998|) , ( Kumpera 
|and Rui^ , P-982| ), and flPasillas-Lepine and Respondek| , |1999bp , after a permutation 
of the Xi's, in the following form: 



%2 
%3 



U{x 1 ,x 2 )u 2 



in-1 — /n-3(^lj • • • > Xn-2) V>2 



X t , 



fn(.Xly • • • ; ^ra— 2) %n— l) V>2, 



where, for 3 < i < n, the f^s are polynomials. Therefore, if we take polynomial 
controls of the form 



Ul {t) 
u 2 {t) 



a + + 
b , 



+ a n ^ 2 t 



n-2 



(12) 



as it has been proposed in ( |Tilbury et al.| , |1995| ) in order to steer the n-trailer around 
regular points, then the control system x = k"(x)mi + K2(x)m 2 can be integrated 
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by successive quadratures, which leads to a system of n polynomial equations 



xj = Pi(a Q , ... , a n _ 2 , 6q, x?, . . . , x°), (13) 



where, for 1 < i < n, the Pj's are polynomial functions of all their arguments and the 
vectors x° and x T denote the initial and final configuration, respectively. Hence, for 
Goursat structures, the nonholonomic motion planning problem can be transformed 
into an algebraic problem: the resolution of a system of polynomial equations. 

In the regular case, the polynomial system (|I3]) is actually a linear system, 
which reflects the flatness of the system around regular configurations (see e.g. 
Fliess et al., 1995; Martin and Rouchon, 1994). Indeed, if we fix i / then the 



final condition x T belongs to a hyperplane Eb of M n , parameterized by the constants 
do, ...,a n _2, and the motion planning problem leads to the resolution of a full-rank 
system of linear equations. Note, however, that this approach fails if bo = 0. That 
is, when the control ([02) produces an abnormal trajectory. 



In the singular case, the system fll~3|) is truly polynomial, which causes at least 
two problems. Firstly, in general, it cannot be solved explicitly and we must use 
numerical approximations. Secondly, although the system is globally controllable, 
there may be restrictions on the set of points accessible with controls of the form ( |12| ) 
and, in general, describing these restrictions leads to another system of polynomial 
equations. 

Fortunately, in small dimension the situation is not so complicated. Indeed, if 
we consider a mobile robot towing two or three trailers then (|13|) leads to a system 
of linear equations together with a single quadratic equation, which can be solved 
easily, and therefore the nonholonomic motion planning problem admits an explicit 
solution. Moreover, the set of points accessible by the family of controls (|12D can 
be completely characterized: if we fix all initial and final angles, the set of points in 
the (£1, ^2)-plane that can be reached from a given configuration is delimited by a 
parabola. 
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5 The Two- Trailer System 



The results presented in this paper can be directly applied to the mobile robot 
towing two trailers, which is given by 



6 


= cos( 


02 


-9 X 


i cos( 


0i 


- ) cos( 


0o)v 2 




= cos( 


2 




I cos( 


0i 


- ) sin( 


%)v 2 




= cos( 


02 


- 0i 


) sin( 


9i 


- #0)^2 




Ox 


= sin( 


% 


-K 










0-2 


= «1- 















We will consider this system in a small enough neighborhood of the singular locus 
02 ~ 0\ — if ? where the transformation 

x x = 6 

X2 = 6 

x 3 = tan(0 o ) (14) 

X4 = sec(6'o) 3 tan(^i — 9 ) 

cos (e ) 4 cos(e 1 -e ) cos Wo— "1) 

X § ~ 3tan(6»o)tan(6»i-6lo)sm(6»i-eo)cos(6»2-6 , i)+scc(e 1 -6» ( ,) 2 (sin(6»2-6 , i)-sin(e 1 -6» ( ,)cos(e2-fi)) 

converts the two-trailer into the following Kumpera-Ruiz normal form: 

±1 = x 5 u 2 
i 2 = U2 
x 3 = X5X4U2 
X4 = U2 
x 5 = u x . 

The feedback transformation between the old and new controls is given in the Matlab 
file system. m of |Pasillas-Lepine| ( J2000| ), which is available by e-mail request to the 
first author. 



We will show that (0) defines a diffeomorphism of a well chosen set V onto R 5 . 
In order to do so, put O = O , 0\ = 0\ — O , 2 = 02 — 0i and represent x 5 , defined 
by (0), as 

a cos 2 , \ 

x 5 = = — - (15) 

b cos 02 + c sin 2 
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where a, b, and c are smooth functions of #0 and 9\. It is easy to observe that for 
any fixed values of #0 and 9\ and on any interval (— it/2 + kir, tt/2 + kir) the function 
b cos 82 + c sin 02 vanishes exactly one time. Let 7(0o, 0i) £ ( — tt/2 + &;7r, 7r/2 + /c7r) 
and 5(90,0!) e (-tt/2 + (fc + l)vr,7r/2 + (fc + 1)tt) be zeros of 6cos0 2 + csin0 2 . We 
claim that for any fixed values of 9q an d 01, the mapflT5|) establishes a diffeomor- 
phism of the interval (7(0 O , 0i), <5(0o, 0i)) onto R. To see this, firstly, observe that 
<5(0o, 0i) — 7(^0; 0i) = 7T- Secondly, dividing, on well chosen sets, the numerator and 
denominator of the right hand side of (fL5|) by sin 2 and replacing cot 02 by —7/ or 
by cos 6*2 and replacing tan0 2 by y, we represent x$, respectively, as restrictions of 
either the homography x 5 = or x 5 = In both cases, derivative of £5 

with respect to 9 2 is of the same sign which is the sign of —ac. This and the fact 
that b cos 9 2 + c sin 9 2 vanishes at 7(0o, 0i) and S(9 , 9%) implies that when 2 changes 
between 7(0o, 0i) an d 8(9 Q , 9{) then the value of X5 either grows monotonically be- 
tween minus and plus infinity or decreases monotonically between plus and minus 
infinity. 

Now define V = {(£1, £2, 0~o, 0~i, 2 ) :^K,( 2 G R, 9 G (-tt/2, tt/2), x G 



(— 7r/2, 7r/2), 2 G (7(6*0, 0i), 5(#0) 0i))}- The above considerations imply that (fUD 
establishes a diffeomorphism between K and M 5 . 

Now fix an initial condition £° = (£1, £2, 0o, 0°) an d a terminal condition 
£ T = {&,&,6%,0[,eZ). Let k be an integer such that 0~° G (7(08,0?), «,0~°)), 
where 7 (0^, 0?) G (-vr/2 + A;vr, vr/2 + A;vr). Assume that 7(0^, 0f) satisfies 7(0^, 0f) G 
(-tt/2 + kn,Tc/2 + A;7r) for the same fc, where Q\ G (7(6^, 0f ), 5(0 o r , 9\ ). Then any 
smooth curve a; (t) in M 5 such that x(0) = £° and x(T) = £ T has a unique smooth 
preimage, with respect to (|TJ). 

The controls u\ and «2 that steer the above Kumpera-Ruiz normal form from 
V = (Pi,P2,P3,P4,P5) at t = to g = (gi, g 2 , 93, 94, 9s) at t = 1 are given in |Pasillas- 



Lcpiiic| ( |2000D (see the files main.m and system. m). Here, we will consider the 
particular case p = (0,0,0,0,0), which is simpler to analyze. If we take a pair of 
polynomial controls of the form 

ui(t) = a 2 + a 3 t + a A t 2 + a 5 t 3 
u 2 (t) = at, 
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then we will obtain: 



xi(l) = ai (|a 2 + |a 3 + -^a 4 + ^a 5 ) 
x 2 (l) = + Yj^ai + + Tc^6 a 5 

7 8 5 

+ 144 a 2 a 3 + 3^50-20-4 + 32o^2 a 5 

+ 32l) a 3 a 4 + 864 a 3 ( 3'5 + 3^5o a 405) 

£3(1) = a\ (|a 2 + |a 3 + ^a 4 + ^a 5 ) 
£4(1) = a x 

£5(1) = «2 + 7^3 + |«4 + \a 5 . 
Now, if we want the final point to be q, we should take: 



ai = q 4 



a 3 = -12a 2 - ^q 3 + + 12g 5 
a4 = 30a 2 + If>g 3 - f^i - 60g 5 
a 5 = -20a 2 - + f% + 60g 5 - 



The constraint x 2 (l) = g 2 gives, for a 2 , the quadratic equation 



55i40^4 «2 + (ife^ - 6§3^^4 + Tlfes^?!) a 2 ~ I§<?1<?3 

2 



It follows that a point g is reachable from zero, with controls of the form (|T6|), if 
and only if: 

92 > 7^4 g? + (-H?3 + izQfas) qi + f I - ^3^5 + ^^I" ( 17 ) 

To steer the two-trailer system: (i) Fix initial and terminal configurations (° 
and ( T that can be joined by a control of the form (P]). In the particular case 
x((°) = 0, the terminal configuration must satisfy flTTD . Check that £° and £ T 
belong to the domain V defined above, (ii) Compute, using the transformation (|14|), 
the initial and final configurations a; = x(C°) and x T = x(( T ) in the coordinates 
of the Kumpera-Ruiz normal form, (iii) Compute a pair of controls («i,w 2 ) of the 
form (|16|), that steer the system from x° to x T . (iv) Apply to the original system 
the controls V\ =d^u\ + rjs w 2 and t> 2 = //5ti 2 , given by (^). For additional details, 
we refer the reader to the three Matlab files main.m, system. m, and chgcoord.m of 
Pasillas-Lepinel (|2000|) . 
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We plot, in Figure 1, the trajectory joining the points (° = (0, 0, 0, 0, |) and ( T = 
(0, 1, 0, f , ^f); and, in Figure 2, the trajectory joining the points (° = (0, 0, 0, — |, 0) 
and ( T = (0,1,0,1,^). In both cases, illustrated by Figures 1 and 2, the initial 
and terminal configurations satisfy the above described conditions which justifies 
the application of our method. Indeed, in the first case we have for the initial 
configuration /3 2 ° = tt/4 so (3° G (7(0, 0), 5(0, 0)), where 7(0,0) = 0, 5(0,0) = tt. 
For the terminal configuration we have /3j = ir/2 e 7(0, 7r/4), 8(0, 7r/4), where 
7(0, 7r/4) = arctan(v^2/4) which obviously satisfies the required condition. Similar 
calculations hold in the case considered in Figure 2. Observe that, in both cases, 
the terminal configuration ( T belongs to the singular locus. 
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Figure 1: (a) Initial and final configurations. Trajectory of the last trailer, (b) From 
the top: the angles 9 , 9 1 and 9 2 . 




Figure 2: (a) Initial and final configurations. Trajectory of the last trailer, (b) From 
the top: the angles 9 , 9\ and 9 2 . 
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